#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

/*
 *	卡尔曼需要建立精确的模型, 然后通过现代控制理论和矩阵运算得出准确的公式
 */

double frand() 
{      
	return 2*((rand()/(double)RAND_MAX) - 0.5);  //随机噪声
}  

void mainz_p()
{	
	float x_last=0;   
	float p_last=0.02;
	float Q=0.018;
	float R=0.542;
	float kg;
	float x_mid;
	float x_now;
	float p_mid;
	float p_now;
	float z_real=0.56;
	float z_measure;
	float sumerror_kalman=0;
	float sumerror_measure=0;
	int i;

	x_last=z_real+frand()*0.03;
	x_mid=x_last;

	for(i=0;i<20;i++)
	{	
		x_mid=x_last;    //x_last=x(k-1|k-1),x_mid=x(k|k-1)
		p_mid=p_last+Q;  //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
		kg=p_mid/(p_mid+R); //kg为kalman filter，R为噪声

		z_measure = z_real+frand()*0.03;//测量值
		x_now = x_mid+kg*(z_measure-x_mid);//估计出的最优值
		p_now = (1-kg)*p_mid;//最优值对应的covariance

		printf("Real     position: %6.3f \n",z_real);  //显示真值
        printf("Mesaured position: %6.3f [diff:%.3f]\n",z_measure,fabs(z_real-z_measure));  //显示测量值以及真值与测量值之间的误差
        printf("Kalman   position: %6.3f [diff:%.3f]\n\n",x_now,fabs(z_real - x_now));  //显示kalman估计值以及真值和卡尔曼估计值的误差

        sumerror_kalman += fabs(z_real - x_now);  //kalman估计值的累积误差
        sumerror_measure += fabs(z_real-z_measure);  //真值与测量值的累积误差

        p_last = p_now;  //更新covariance值
        x_last = x_now;  //更新系统状态值
	}

	printf("总体测量误差      : %f\n",sumerror_measure);  //输出测量累积误差
    printf("总体卡尔曼滤波误差: %f\n",sumerror_kalman);   //输出kalman累积误差
    printf("卡尔曼误差所占比例: %d%% \n",100-(int)((sumerror_kalman/sumerror_measure)*100)); 
}


void KalmanFilter_t(const double ResrcData[], int len, double ProcessNiose_Q,double MeasureNoise_R)
{	
	float x_last=0;   
	float p_last=0.02;
	float Q=ProcessNiose_Q;
	float R=MeasureNoise_R;
	float kg;
	float x_mid;
	float x_now;
	float p_mid;
	float p_now;
	float z_real=4.4; // 4.3234;
	float z_measure;
	float sumerror_kalman=0;
	float sumerror_measure=0;
	int i;

	x_last=z_real;
	x_mid=x_last;

	for(i=0;i<len;i++)
	{	
		x_mid=x_last;    //x_last=x(k-1|k-1),x_mid=x(k|k-1)
		p_mid=p_last+Q;  //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
		kg=p_mid/(p_mid+R); //kg为kalman filter，R为噪声

		z_measure = ResrcData[i];//测量值
		x_now = x_mid+kg*(z_measure-x_mid);//估计出的最优值
		p_now = (1-kg)*p_mid;//最优值对应的covariance

		printf("Real     position: %6.3f \n",z_real);  //显示真值
        printf("Mesaured position: %6.3f [diff:%.3f]\n",z_measure,fabs(z_real-z_measure));  //显示测量值以及真值与测量值之间的误差
        printf("Kalman   position: %6.3f [diff:%.3f]\n\n",x_now,fabs(z_real - x_now));  //显示kalman估计值以及真值和卡尔曼估计值的误差

        sumerror_kalman += fabs(z_real - x_now);  //kalman估计值的累积误差
        sumerror_measure += fabs(z_real-z_measure);  //真值与测量值的累积误差

        p_last = p_now;  //更新covariance值
        x_last = x_now;  //更新系统状态值
	}

	printf("总体测量误差      : %f\n",sumerror_measure);  //输出测量累积误差
    printf("总体卡尔曼滤波误差: %f\n",sumerror_kalman);   //输出kalman累积误差
    printf("卡尔曼误差所占比例: %d%% \n",100-(int)((sumerror_kalman/sumerror_measure)*100)); 
}


/*       
 * 三个值需要根据不同的实现来尝试调节。
 *
 * Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏
 * R:测量噪声，R增大，动态响应变慢，收敛稳定性变好       
*/
double KalmanFilter(const double ResrcData,
                                        double ProcessNiose_Q,double MeasureNoise_R)
{
        double R = MeasureNoise_R;
        double Q = ProcessNiose_Q;

        static        double x_last;

        double x_mid = x_last;
        double x_now;

        static        double p_last;
		//double p_last = 0.02;

        double p_mid ;
        double p_now;
        double kg;       

        x_mid = x_last;				// x_last=x(k-1|k-1),x_mid=x(k|k-1)
        p_mid = p_last + Q;			// p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
        kg = p_mid / (p_mid + R);	// kg为kalman filter，R为噪声

        x_now = x_mid + kg * (ResrcData - x_mid);	// 估计出的最优值               
        p_now=(1 - kg) * p_mid;		// 最优值对应的covariance   
		
		//printf("Real     position: %6.6f \n",ResrcData);  //显示真值
        //printf("Kalman   position: %6.6f [diff:%.3f]\n\n",x_now,fabs(ResrcData - x_now));  //显示kalman估计值以及真值和卡尔曼估计值的误差

        p_last = p_now;				// 更新covariance值
        x_last = x_now;				// 更新系统状态值

        return x_now;               
}

void main()
{
	int i;
	double now;

	double x[62] = {2.539126,2.546689,2.508891,2.553887,2.541768,2.533591,2.571051,2.530253,2.562923,2.543315,2.543315,2.530653,2.570505,2.524798,
				2.572993,2.557545,2.520041,2.528175,2.547057,2.552768,2.509579,2.551065,2.555673,2.530524,2.535884,2.534815,2.543681,
				2.493674,2.531325,2.547425,2.526577,2.544379,2.512001,2.549040,2.526818,2.545293,2.542055,2.571055,2.539378,2.554990,
				2.522171,2.578157,2.556790,2.530834,2.564409,2.556652,2.563419,2.525953,2.591824,2.531374,2.531374,2.531374,2.531374,
				2.531374,2.531374,2.531374,2.531374,2.531374,2.531374,2.531374,2.531374,2.531374};

	double y[48] = {1.015123,1.009422,1.029001,1.030982,1.018198,1.059065,1.054612,1.048085,0.982232,1.022043,
					1.009422,1.045628,1.058952,1.054677,1.028388,1.020677,1.033453,1.013755,1.022043,1.018202,1.017104,1.022043,
					1.018202,1.005588,1.020182,1.023198,1.055597,1.010655,1.034352,1.050718,1.014360,0.996720,1.050081,1.047606,
					1.003608,1.015096,1.015096,1.015096,1.015096,1.015096,1.015096,1.015096,1.015096,1.015096,1.015096,1.015096,
					1.015096,1.015096};


	double negative_y[48] = {-1.015123,-1.009422,-1.029001,-1.030982,-1.018198,-1.059065,-1.054612,-1.048085,-0.982232,
							-1.022043,-1.009422,-1.045628,-1.058952,-1.054677,-1.028388,-1.020677,-1.033453,-1.013755,-1.022043,-1.018202,
							-1.017104,-1.022043,-1.018202,-1.005588,-1.020182,-1.023198,-1.055597,-1.010655,-1.034352,-1.050718,-1.014360,
							-0.996720,-1.050081,-1.047606,-1.003608,-1.015096,-1.015096,-1.015096,-1.015096,-1.015096,-1.015096,-1.015096,
							-1.015096,-1.015096,-1.015096,-1.015096,-1.015096,-1.015096};



	
	double src_data[30] = {4.430786, 4.432327, 4.440811,4.444108,4.445648,4.446581,4.449042,
							4.450294, 4.451193,4.452515,4.454644,4.458331,4.458989,4.462853,
							4.462853,4.463066,4.463066,4.465615,4.46596,4.468468,4.469675,
							4.473649,4.475337,4.481661,4.484437,4.490076,4.492028,4.492616,
							4.513689,4.516462};
	
	#if 0
	KalmanFilter_t(src_data, 30, 0.018, 4.282);
	#endif

	#if 1
	for (i = 0; i < 30; i++) {
		now = KalmanFilter(src_data[i], 0.01, 0.01);
		printf("%f\n", now);
	}
	#endif
}



